
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       takeoff.c
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// start takeoff to specified altitude above home in centimeters
void takeoff_start(UserTakeOff* takeoff, float alt_cm)
{
    // initialise takeoff state
    takeoff->_running = true;
    takeoff->take_off_start_alt = posctrl_get_pos_target_z_cm(fms.pos_control);
    takeoff->take_off_complete_alt  = takeoff->take_off_start_alt + alt_cm;
}

// stop takeoff
void takeoff_stop(UserTakeOff* takeoff)
{
    takeoff->_running = false;

    // Check if we have progressed far enough through the takeoff process that the
    // aircraft may have left the ground but not yet detected the climb.
    if (attctrl_get_throttle_in(fms.attitude_control) > fms_get_non_takeoff_throttle()) {
        fms_set_land_complete(false);
    }
}

// do_pilot_takeoff - controls the vertical position controller during the process of taking off
//  take off is complete when the vertical target reaches the take off altitude.
//  climb is cancelled if pilot_climb_rate_cm becomes negative
//  sets take off to complete when target altitude is within 1% of the take off altitude
void takeoff_do_pilot_takeoff(UserTakeOff* takeoff, float pilot_climb_rate_cm)
{
    // return pilot_climb_rate if take-off inactive
    if (!takeoff->_running) {
        return;
    }

    if (fms.ap.land_complete) {
        // send throttle to attitude controller with angle boost
        float throttle = math_constrain_float(attctrl_get_throttle_in(fms.attitude_control) + fms.G_Dt / fms.g.takeoff_throttle_slew_time, 0.0, 1.0);
        attctrl_set_throttle_out(fms.attitude_control, throttle, true, 0.0);
        // tell position controller to reset alt target and reset I terms
        posctrl_init_z_controller(fms.pos_control);
        if (throttle >= 0.9 || 
            (posctrl_get_z_accel_cmss(fms.pos_control) >= 0.5 * posctrl_get_max_accel_z_cmss(fms.pos_control)) ||
            (posctrl_get_vel_desired_cms(fms.pos_control).z >= math_constrain_float(pilot_climb_rate_cm, posctrl_get_max_speed_up_cms(fms.pos_control) * 0.1, posctrl_get_max_speed_up_cms(fms.pos_control) * 0.5f)) || 
            (math_flt_positive(takeoff->take_off_complete_alt - takeoff->take_off_start_alt) && posctrl_get_pos_target_z_cm(fms.pos_control) - takeoff->take_off_start_alt > 0.5 * (takeoff->take_off_complete_alt - takeoff->take_off_start_alt))) {
            // throttle > 90%
            // acceleration > 50% maximum acceleration
            // velocity > 10% maximum velocity && commanded climb rate
            // velocity > 50% maximum velocity
            // altitude change greater than half complete alt from start off alt
            fms_set_land_complete(false);
        }
    } else {
        float pos_z = takeoff->take_off_complete_alt;
        float vel_z = pilot_climb_rate_cm;

        // command the aircraft to the take off altitude and current pilot climb rate
        posctrl_input_pos_vel_accel_z(fms.pos_control, &pos_z, &vel_z, 0, true);

        // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude
        if (math_flt_negative(pilot_climb_rate_cm) ||
            (takeoff->take_off_complete_alt  - takeoff->take_off_start_alt) * 0.999f < posctrl_get_pos_target_z_cm(fms.pos_control) - takeoff->take_off_start_alt) {
            takeoff_stop(takeoff);
        }
    }
}

bool takeoff_triggered(const float target_climb_rate)
{
    if (!fms.ap.land_complete) {
        // can't take off if we're already flying
        return false;
    }
    if (target_climb_rate <= 0.0f) {
        // can't takeoff unless we want to go up...
        return false;
    }

    if (fms.motors->_spool_state != MB_MOTOR_THROTTLE_UNLIMITED) {
        // hold aircraft on the ground until rotor speed runup has finished
        return false;
    }

    return true;
}

/*------------------------------------test------------------------------------*/


